// frame.c - frame matrix utility functions
// ----------------------------------------

#include <math.h>
#include "mmath.h"
#include "frame.h"

#define PI  3.14159265358979323846
#define EPS 0.000001

// build the body-to-navigation matrix from Euler angles (allocates matrix)
double** euler_angles_to_body_to_nav_matrix(double* euler) {
	double** Cbn = dmatalloc(3, 3);
	Cbn[0][0] = cos(euler[1])*cos(euler[2]);
	Cbn[0][1] = -cos(euler[0])*sin(euler[2]) + sin(euler[0])*sin(euler[1])*cos(euler[2]);
	Cbn[0][2] = sin(euler[0])*sin(euler[2]) + cos(euler[0])*sin(euler[1])*cos(euler[2]);
	Cbn[1][0] = cos(euler[1])*sin(euler[2]);
	Cbn[1][1] = cos(euler[0])*cos(euler[2]) + sin(euler[0])*sin(euler[1])*sin(euler[2]);
	Cbn[1][2] = -sin(euler[0])*cos(euler[2]) + cos(euler[0])*sin(euler[1])*sin(euler[2]);
	Cbn[2][0] = -sin(euler[1]);
	Cbn[2][1] = sin(euler[0])*cos(euler[1]);
	Cbn[2][2] = cos(euler[0])*cos(euler[1]);
	return Cbn;
}

// extract the Euler angles from the body-to-navigation matrix (allocates matrix)
double* body_to_nav_matrix_to_euler_angles(double** Cbn) {
	double* euler = dvecalloc(3);
	_body_to_nav_matrix_to_euler_angles(euler, Cbn);
	return euler;
}

void _body_to_nav_matrix_to_euler_angles(double* euler, double** Cbn) {
	euler[0] = atan2(Cbn[2][1], Cbn[2][2]);
	euler[2] = atan2(Cbn[1][0], Cbn[0][0]);
	if (abs(Cbn[2][0]+1) < EPS) euler[1] = -PI/2;
	else if (abs(Cbn[2][0]-1) < EPS) euler[1] = PI/2;
	else euler[1] = -atan(Cbn[2][0] / sqrt(1 - Cbn[2][0]*Cbn[2][0]));
}

// convert body-to-navigation matrix to quaternion (allocates vector quaternion)
double* body_to_nav_matrix_to_quaternion(double** Cbn) {
	double* q = dvecalloc(4);
	double mag = sqrt(1 + Cbn[0][0] + Cbn[1][1] + Cbn[2][2]);
	q[0] = 0.5 * (Cbn[2][1] - Cbn[1][2]) / mag;
	q[1] = 0.5 * (Cbn[0][2] - Cbn[2][0]) / mag;
	q[2] = 0.5 * (Cbn[1][0] - Cbn[0][1]) / mag;
	q[3] = 0.5 * mag;
	return q;
}

// convert quaternion to body-to-navigation matrix (allocates matrix)
double** quaternion_to_body_to_nav_matrix_shin(double* q) {
	double** Cbn = dmatalloc(3, 3);
	_quaternion_to_body_to_nav_matrix_shin(Cbn, q);
	return Cbn;
}

// convert quaternion to body-to-navigation matrix (modifies existing matrix) as developed by Shin
void _quaternion_to_body_to_nav_matrix_shin(double** Cbn, double* q) {
	double q00 = q[0]*q[0];
	double q11 = q[1]*q[1];
	double q22 = q[2]*q[2];
	double q33 = q[3]*q[3];
	double q01 = q[0]*q[1];
	double q02 = q[0]*q[2];
	double q03 = q[0]*q[3];
	double q12 = q[1]*q[2];
	double q13 = q[1]*q[3];
	double q23 = q[2]*q[3];
	Cbn[0][0] = q00 - q11 - q22 + q33;
	Cbn[0][1] = 2 * (q01 - q23);
	Cbn[0][2] = 2 * (q02 - q13);
	Cbn[1][0] = 2 * (q01 + q23);
	Cbn[1][1] = q11 - q00 - q22 + q33;
	Cbn[1][2] = 2 * (q12 - q03);
	Cbn[2][0] = 2 * (q02 - q13);
	Cbn[2][1] = 2 * (q12 + q03);
	Cbn[2][2] = q22 - q00 - q11 + q33;
}

// convert quaternion to body-to-navigation matrix (allocates matrix)
double** quaternion_to_body_to_nav_matrix_eskin(double* q) {
	double** Cbn = dmatalloc(3, 3);
	_quaternion_to_body_to_nav_matrix_eskin(Cbn, q);
	return Cbn;
}

// convert quaternion to body-to-navigation matrix (modifies existing matrix) as developed by Eskin
void _quaternion_to_body_to_nav_matrix_eskin(double** Cbn, double* q) {
	double q00 = q[0]*q[0];
	double q11 = q[1]*q[1];
	double q22 = q[2]*q[2];
	double q33 = q[3]*q[3];
	double q01 = q[0]*q[1];
	double q02 = q[0]*q[2];
	double q03 = q[0]*q[3];
	double q12 = q[1]*q[2];
	double q13 = q[1]*q[3];
	double q23 = q[2]*q[3];
	Cbn[0][0] = q00 - q11 - q22 + q33;
	Cbn[0][1] = 2 * (q01 + q23);
	Cbn[0][2] = 2 * (q02 - q13);
	Cbn[1][0] = 2 * (q01 - q23);
	Cbn[1][1] = q11 - q00 - q22 + q33;
	Cbn[1][2] = 2 * (q12 + q03);
	Cbn[2][0] = 2 * (q02 + q13);
	Cbn[2][1] = 2 * (q12 - q03);
	Cbn[2][2] = q22 - q00 - q11 + q33;
}

// build the quaternion update matrix (allocates matrix)
double** quaternion_update_matrix_shin(double* dtheta) {
	double** qum = dmatalloc(4, 4);
	_quaternion_update_matrix_shin(qum, dtheta);
	return qum;
}

// build the quaternion update matrix (modifies existing matrix) as developed by Shin
void _quaternion_update_matrix_shin(double** qum, double* dtheta) {
	double mag = sqrt(dtheta[0]*dtheta[0] + dtheta[1]*dtheta[1] + dtheta[2]*dtheta[2]);
	double s = (mag == 0)? 0 : sin(mag/2)/mag;
	double c = cos(mag/2) - 1;
	double sdx = s*dtheta[0];
	double sdy = s*dtheta[1];
	double sdz = s*dtheta[2];
	qum[0][0] = c;
	qum[0][1] = sdz;
	qum[0][2] = -sdy;
	qum[0][3] = sdx;
	qum[1][0] = -sdz;
	qum[1][1] = c;
	qum[1][2] = sdx;
	qum[1][3] = sdy;
	qum[2][0] = sdy;
	qum[2][1] = -sdx;
	qum[2][2] = c;
	qum[2][3] = sdz;
	qum[3][0] = -sdx;
	qum[3][1] = -sdy;
	qum[3][2] = -sdz;
	qum[3][3] = c;
}

// build the quaternion update matrix (allocates matrix)
double** quaternion_update_matrix_eskin(double* dtheta) {
	double** qum = dmatalloc(4, 4);
	_quaternion_update_matrix_eskin(qum, dtheta);
	return qum;
}

// build the quaternion update matrix (modifies existing matrix) as developed by Eskin
void _quaternion_update_matrix_eskin(double** qum, double* dtheta) {
	double mag = sqrt(dtheta[0]*dtheta[0] + dtheta[1]*dtheta[1] + dtheta[2]*dtheta[2]);
	double s = (mag == 0)? 0 : sin(mag/2)/mag;
	double c = cos(mag/2);
	double sdx = s*dtheta[0];
	double sdy = s*dtheta[1];
	double sdz = s*dtheta[2];
	qum[0][0] = c;
	qum[0][1] = sdz;
	qum[0][2] = -sdy;
	qum[0][3] = sdx;
	qum[1][0] = -sdz;
	qum[1][1] = c;
	qum[1][2] = sdx;
	qum[1][3] = sdy;
	qum[2][0] = sdy;
	qum[2][1] = -sdx;
	qum[2][2] = c;
	qum[2][3] = sdz;
	qum[3][0] = -sdx;
	qum[3][1] = -sdy;
	qum[3][2] = -sdz;
	qum[3][3] = c;
}

// build the sculling matrix (allocates new matrix)
double** sculling_matrix(double* dtheta) {
	double** sm = dmatalloc(3, 3);
	sm[0][0] = 1;
	sm[1][1] = 1;
	sm[2][2] = 1;
	_sculling_matrix(sm, dtheta);
	return sm;
}

// build the sculling matrix (modifies existing matrix)
// *** ASSUMES DIAGONAL IS UNITY ***
void _sculling_matrix(double** sm, double* dtheta) {
	sm[0][1] =  0.5 * dtheta[2];
	sm[0][2] = -0.5 * dtheta[1];
	sm[1][0] = -0.5 * dtheta[2];
	sm[1][2] =  0.5 * dtheta[0];
	sm[2][0] =  0.5 * dtheta[1];
	sm[2][1] = -0.5 * dtheta[0];
}
